
#include "mlx90393driver.h"
#include "math.h"

uint16_t XDATA=0,YDATA=0,ZDATA=0;
uint8_t Mdata[6] = {0};
uint8_t StateReg = 0;

/**
 * 由于使用SPI单线模式，即数据线只有一条，
 * 进行发送和接受信息时，需要配置发送引脚的模式，使不冲突
 */
#define SPI_TX_OFF()            \
	do                            \
	{                             \
		GPIOA->MODER &= 0xFFFF3FFF; \
		GPIOA->MODER |= 0x00000000; \
	} while (0) // PA7--MOSI输入模式

#define SPI_TX_ON()             \
	do                            \
	{                             \
		GPIOA->MODER &= 0xFFFF3FFF; \
		GPIOA->MODER |= 0x00008000; \
	} while (0) // PA7--MOSI复用

#define MLX90393_CS_ENABLE() HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_RESET)
#define MLX90393_CS_DISABLE() HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET)

uint8_t write_readonebyte(SPIInstance *spi_instance, uint8_t txdata)
{
    uint8_t rx_data;
    HAL_SPI_TransmitReceive(spi_instance->spi_handle, &txdata, &rx_data, 1, 1000);
    return rx_data;
}

/**
  * @brief MLX90393 warm reset function
  * @param in_data: Buffer for incoming data
  * @retval None
  */
void MLX90393_Reset(SPIInstance *spi_instance, uint8_t * in_data)
{
    HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_RESET);
    write_readonebyte(spi_instance, MLX_RESET);
    StateReg = write_readonebyte(spi_instance, 0x00);
    HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_SET);
    DWT_Delay(0.015);
}

/**
  * @brief MLX90393 SPI Write command Function
  * @param command: Command to write
  * @param in_data: Buffer for incoming data
  * @retval None
  */
void MLX90393_WriteCommand(SPIInstance *spi_instance, uint8_t command, uint8_t * in_data)
{	
    HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_RESET);
    write_readonebyte(spi_instance, command);
    StateReg = write_readonebyte(spi_instance, 0x00);
    HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_SET);
    DWT_Delay(0.015);
}

/**
  * @brief MLX90393 SPI Read register Function
  * @param data: Content of register
  * @param addr: Register address
  * @param in_data: Buffer for incoming data
  * @retval None
  */
void MLX90393_ReadRegister(SPIInstance *spi_instance, uint8_t addr , uint8_t * in_data)
{	
	HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_RESET);
    write_readonebyte(spi_instance, 0x50);
    write_readonebyte(spi_instance, addr<<2);
    StateReg = write_readonebyte(spi_instance, 0x00);
    *(in_data) = write_readonebyte(spi_instance, 0x00);
    *(in_data+1) = write_readonebyte(spi_instance, 0x00);
    HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_SET);
    DWT_Delay(0.015);
}

/**
  * @brief MLX90393 SPI Write register Function
  * @param data: Content of register
  * @param addr: Register address
  * @param in_data: Buffer for incoming data
  * @retval None
  */
void MLX90393_WriteRegister(SPIInstance *spi_instance, uint16_t data,  uint8_t addr , uint8_t * in_data)
{	
	HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_RESET);
    write_readonebyte(spi_instance, 0x60);
    write_readonebyte(spi_instance, data >> 8);
    write_readonebyte(spi_instance, data & 0xFF);
    write_readonebyte(spi_instance, addr<<2);
    StateReg = write_readonebyte(spi_instance, 0x00);
    HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_SET);
    DWT_Delay(0.015);
}

uint8_t MLX90393_Start(SPIInstance *spi_instance)
{   
    uint8_t rxdata[5]={0};
    uint16_t tmp_data=0;

    // exit
    MLX90393_WriteCommand(spi_instance, MLX_EXIT, rxdata);
    if (StateReg == 0xFF){
        return StateReg;
    }
    // DWT_Delay(0.1);
    // reset
    MLX90393_Reset(spi_instance, rxdata);
    if (StateReg == 0xFF){
        return StateReg;
    }
    // DWT_Delay(0.1);
    // Register 0
    // MLX90393_ReadRegister(0x00, rxdata);
	tmp_data = 0X0000;
    MLX90393_WriteRegister(spi_instance, tmp_data, 0x00, rxdata);
    if (StateReg == 0xFF){
        return StateReg;
    }
    // Register 1
    // MLX90393_ReadRegister(0x01, rxdata);
    tmp_data = 0x0000; // Set register value
    MLX90393_WriteRegister(spi_instance, tmp_data, 0x01, rxdata);
    if (StateReg == 0xFF){
        return StateReg;
    }
    // Register 2
    // MLX90393_ReadRegister(0x02, rxdata);
	tmp_data = 0X01E5;
    MLX90393_WriteRegister(spi_instance, tmp_data, 0x02, rxdata);
    if (StateReg == 0xFF){
        return StateReg;
    }
    // Burst mode XYZ
    MLX90393_WriteCommand(spi_instance, 0x16, rxdata);

    HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_RESET);
    write_readonebyte(spi_instance, 0x46);
	StateReg = write_readonebyte(spi_instance, 0x00);
	Mdata[0] = write_readonebyte(spi_instance, 0x00);
	Mdata[1] = write_readonebyte(spi_instance, 0x00);
	Mdata[2] = write_readonebyte(spi_instance, 0x00);
	Mdata[3] = write_readonebyte(spi_instance, 0x00);
    HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_SET);

    return StateReg;
}

/**
  * @brief MLX90393 get measured data
  * @param data: variable for storing data
  * @param sensor: Sensor struct
  * @retval status
  */
void MLX90393_GetData(SPIInstance *spi_instance, float angle[3])
{
    DWT_Delay(0.03);
    HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_RESET);
    write_readonebyte(spi_instance, 0x46);
	StateReg = write_readonebyte(spi_instance, 0x00);
	Mdata[0] = write_readonebyte(spi_instance, 0x00);
	Mdata[1] = write_readonebyte(spi_instance, 0x00);
	Mdata[2] = write_readonebyte(spi_instance, 0x00);
	Mdata[3] = write_readonebyte(spi_instance, 0x00);
    HAL_GPIO_WritePin(spi_instance->GPIOx, spi_instance->cs_pin, GPIO_PIN_SET);
    if (StateReg == 0x91){
        XDATA=(Mdata[0]<<8)+Mdata[1];
	    YDATA=(Mdata[2]<<8)+Mdata[3];
    }

    angle[0] = atan2f((1.0f*YDATA-16384), (1.0f*XDATA-16384));
}
